#!/usr/bin/env python
#################################################################################
# Copyright 2018 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#################################################################################

# author:wanghan #
#nishizhen wei zheng, 0-360

import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from math import radians, copysign, sqrt, pow, pi, atan2,cos,sin
from tf.transformations import euler_from_quaternion
import numpy as np
from sensor_msgs.msg import LaserScan

msg = """
control your Turtlebot3!
-----------------------
this is follower1
-----------------------
"""
tb_leader_pos=Point()
tb_follower1_pos=Point()
tb_follower2_pos=Point()
leader_vel=Twist()
tb_follower1_vel=Twist()
old_pos_error=Point()
new_pos_error=Point()

leader_pos=Point()

fai_exp=pi/4
rou_exp=0.5
K1=0.2
K2=0.3
L=0.1




K3=2
K4=1.9
detect_R=0.4
communicate_R=1.2
vmax=0.3
#this max_vel is just defined casually

safe_r=0.3
ID=1
dx=-0.6
dy=0.6
period=0.1

time=0
data_x=[0]*100
index=0
data_y=[0]*100


class GotoPoint():
    def __init__(self):
        rospy.init_node('follower1', anonymous=False)

        rospy.on_shutdown(self.shutdown)
        self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        self.tb_follower1_positon=rospy.Publisher('/tb_follower1_pos',Point,queue_size=5)
        position = Point()                          
        move_cmd = Twist()
        r = rospy.Rate(10)

        self.tf_listener = tf.TransformListener()
        self.odom_frame = '/follower1/odom'
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/follower1/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/follower1/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:

                self.tf_listener.waitForTransform(self.odom_frame, '/follower1/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/follower1/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between odom and base_link or base_footprint")
                rospy.signal_shutdown("tf Exception")
        # print 66
        (position, rotation) = self.get_odom()
        
        rospy.Subscriber('/tb_leader_pos',Point,point_callback_1)
        # rospy.Subscriber('/tb_follower1_pos',Point,point_callback_2)
        rospy.Subscriber('/tb_follower2_pos',Point,point_callback_3)
        rospy.Subscriber('/tb_leader_vel',Twist,vel_callback_4)
        self.tb_follower1_positon.publish(position)
        # print 67
        global leader_vel
        global tb_leader_pos
        global tb_follower1_pos
        global tb_follower2_pos 
        global tb_follower1_vel
        global index
        global time
        global data_x
        global data_y
        global leader_pos
        # get values
        ########################################################################
        # subscribe messages
        




        # get values from sensor
        rospy.Subscriber('/tb_leader_vel',Twist,vel_callback_4)
        fai_L=atan2(leader_vel.linear.y,leader_vel.linear.x)
        follower1_vel=Twist()


        



        #calculate value control algorithm
        ########################################################################
        delta_fai = fai_L-fai_F
















        ########################################################################
        delta_x=(tb_leader_pos.x-position.x+dx)
        delta_y=(tb_leader_pos.y-position.y+dy)

        avoid_delta=0
        tb_follower1_pos=position

        obstacle_pos=self.lidar(position)
        r=sqrt(pow(obstacle_pos.x-position.x,2)+pow(obstacle_pos.y-position.y,2))
        communicate_r=sqrt(pow(tb_leader_pos.x-position.x,2)+pow(tb_leader_pos.y-position.y,2))
        print 10



        if r<detect_R and (obstacle_pos.z<90 or obstacle_pos.z>270):
            print 11

            beta=atan2(obstacle_pos.y-position.y,obstacle_pos.x-position.x)
            theta2=self.compute_theta(beta,rotation)
            print ('follower 1 theta2=%f'%(theta2))
            print ('follower 1 detect_ang=%f'%(obstacle_pos.z))
            print obstacle_pos.x
            print position.x
            tb_follower1_vel.angular.z=0.1*1/(theta2+1)
            tb_follower1_vel.linear.x=0.1
            leader_pos=tb_leader_pos
            print 1
            self.cmd_vel.publish(tb_follower1_vel) 

        if communicate_r<=communicate_R and (r>detect_R or (obstacle_pos.z>90 and obstacle_pos.z<270)):
            print 7
            time=0
            theta=tb_leader_pos.z-rotation
            tb_follower1_vel.linear.x=K0*tb_leader_vel.linear.x*cos(theta)+K1*delta_x
            print ('follower 1 velecity=%f'%(tb_follower1_vel.angular.z))
            tb_follower1_vel.angular.z=K0*tb_leader_vel.angular.z+K2*theta+K4*tb_leader_vel.linear.x*delta_y*(sin(theta)/theta)
            #tb_follower1_vel.linear.x=K0*tb_leader_vel.linear.x+K1*(delta_x*cos(theta)+delta_y*sin(theta))
            #print ('follower 1 velecity=%f'%(tb_follower1_vel.linear.x))

            #tb_follower1_vel.angular.z=K0*tb_leader_vel.angular.z+K2*atan2(delta_y,delta_x)
            print 2
            self.cmd_vel.publish(tb_follower1_vel)
        if communicate_r>communicate_R:
        	print 8
        	if r<detect_R and (obstacle_pos.z<90 or obstacle_pos.z>270): 
        		time=time+0.1
        		print 3

        	else:
        		leader_vel_x=estimate(data_x)
        		leader_vel_y=estimate(data_y)
        		print leader_vel_x
        		leader_pos_x=tb_leader_pos.x+time*leader_vel_x
        		leader_pos_y=tb_leader_pos.y+time*leader_vel_y
        		aerfa1=atan2(leader_vel_y,leader_vel_x)
        		aerfa2=atan2((leader_vel_y-position.y),(leader_vel_x-position.x))
        		aerfa=aerfa1-aerfa2
        		v=sqrt(pow(leader_vel_y,2)+pow(leader_vel_x,2))
        		a=sqrt(pow(leader_pos.y-position.y,2)+pow(leader_pos.x-position.x,2))
        		t=(-2*v*a*cos(aerfa)+sqrt(4*pow(v,2)*pow(a,2)*pow(cos(aerfa),2)+4*pow(a,2)*(pow(vmax,2)-pow(v,2))))/(2*(pow(vmax,2)-pow(v,2)))
        		goaly=leader_pos_y+leader_vel_y*t
        		goalx=leader_pos_x+leader_vel_x*t
        		delta_y=goaly-position.y
        		delta_x=goalx-position.x
        		print ('a=%f'%(a))
        		print ('aerfa=%f'%(aerfa))
        		print ('follower1s position=%f'%(position.x))
        		print('v=%f'%(v))
        		print ('goalx=%f'%(goalx))
        		print ('t=%f'%(t))
        		print ('communicate_r=%f'%(communicate_r))
        		
        		beta=atan2((goaly-position.y),(goalx-position.x))
        		print beta
        		print rotation
        		if(abs(beta-rotation))<0.1:
        			tb_follower1_vel.angular.z=0
        		##set a threshold
        		else:
        			tb_follower1_vel.angular.z=K3*(beta-rotation)

        		if(tb_follower1_vel.angular.z==0):
        			tb_follower1_vel.linear.x=vmax
        		else :
        			tb_follower1_vel.linear.x=0

        		print tb_follower1_vel
        		self.cmd_vel.publish(tb_follower1_vel)



        print 4
        print index





        if communicate_r<communicate_R:
	        if index<100:
	        	data_x[index]=tb_leader_pos.x
	        	data_y[index]=tb_leader_pos.y
	        	index=index+1 
	        else:
	        	data_x[99]=tb_leader_pos.x
	        	data_y[99]=tb_leader_pos.y
	        	for j in range(0,99):
	        		data_x[j]=data_x[j+1]
	        		data_y[j]=data_y[j+1]
        print data_x
        f1.write(str(data_x))
        f1.write('\n')

    
    def lidar(self,tb_pos):
        msg = rospy.wait_for_message("scan", LaserScan)
        LIDAR_ERR = 0.05
        LIDAR_MAX = 2
        obstacle=[]
        min_dis=3
        min_ang=0
        min_point=Point()
        for i in range(360):
            if i <= 90 or i > 270 and i!=0:
                obstacle_pos=Point()
                if msg.ranges[i] >= LIDAR_ERR and msg.ranges[i]<=LIDAR_MAX and msg.ranges[i]>0.17:
                    obstacle_pos.x=tb_pos.x+msg.ranges[i]*cos(i*2*pi/360)
                    obstacle_pos.y=tb_pos.y+msg.ranges[i]*sin(i*2*pi/360)
                    obstacle.append(obstacle_pos)
                    if msg.ranges[i] < min_dis:
                            min_dis = msg.ranges[i]
                            min_ang = i
        if min_dis<3:
            min_point.x=tb_pos.x+min_dis*cos(i*2*pi/360)
            min_point.y=tb_pos.y+min_dis*sin(i*2*pi/360)
            min_point.z=min_ang
        else:
            min_point.x=-1000
            min_point.y=-1000
        return min_point



    def compute_theta(self,theta,rotation1):
        theta=theta%(2*pi)
        if theta>pi:
            theta=theta-2*pi
        rotation1=rotation1%(2*pi)
        if rotation1>pi:
            rotation1=rotation1-2*pi
        w=theta-rotation1
        return w
        


    def get_odom(self):
        try:
            (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
            rotation = euler_from_quaternion(rot)

        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), rotation[2])
    def shutdown(self):
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
       
def point_callback_1(data):
    global tb_leader_pos
    tb_leader_pos.x=data.x
    tb_leader_pos.y=data.y
    tb_leader_pos.z=data.z
def point_callback_2(data):
    global tb_follower1_pos
    tb_follower1_pos.x=data.x
    tb_follower1_pos.y=data.y
def point_callback_3(data):
    global tb_follower2_pos	
    tb_follower2_pos.x=data.x
    tb_follower2_pos.y=data.y
def vel_callback_4(data):
    global leader_vel
    leader_vel=data
def estimate(data):
	sum_low=0
	sum_high=0
	for j in range(49):
		sum_low=sum_low+data[j]
	for j in range(50,99):
		sum_high=sum_high+data[j]
	average=(sum_high-sum_low)/(50*50*0.1)

	return average






if __name__ == '__main__':
    f1=open("/home/iwin1/catkin_ws/src/president/scripts/data/posx.txt","r+")
    try:
	    
        
        old_pos_error.x=0
        old_pos_error.y=0
        new_pos_error.x=0
        new_pos_error.y=0
        
        while not rospy.is_shutdown():
            print(msg)
            GotoPoint()

    except:
        rospy.loginfo("shutdown program.")
